#include <iostream>
#include "png_pcd_path.hpp"

void pLine() {
    std::cout << "--------------------" << std::endl;
}

int main(int argc, char* argv[]) {
    if (argc < 2) {
        std::cerr << "用法: " << argv[0] << " <点云文件路径>" << std::endl;
        return -1;
    }

    std::string input_file = argv[1];
    std::cout << "=== 鞋底打胶路径算法开始 ===" << std::endl;

    // 创建算法对象
    ShoeSolePathAlgorithm algorithm;

    // 0. 加载参数文件
    if (!algorithm.loadParametersFromFile("../param.yaml")) {
        std::cerr << "参数加载失败，将使用默认参数" << std::endl;
    }

    // 打印当前参数设置
    // algorithm.printParameters();

    // 1. 加载点云
    if (!algorithm.loadPointCloud(input_file)) {
        return -1;
    }
    pLine();
    // 2. 应用直通滤波（使用参数文件）
    if (!algorithm.applyPassThroughFilter()) {
        return -1;
    }
    pLine();
    // 3. 应用体素滤波（使用参数文件）
    if (!algorithm.applyVoxelFilter()) {
        return -1;
    }
    pLine();
    // 4. 应用kNN滤波（使用参数文件）
    if (!algorithm.applyKNNFilter()) {
        return -1;
    }
    pLine();
    // 4.5. 应用欧式聚类，只保留最大聚类（使用参数文件）
    if (!algorithm.applyEuclideanClustering()) {
        return -1;
    }
    pLine();
    // 5. 提取鞋底边缘（使用参数文件）
    if (!algorithm.extractSoleBoundary()) {
        return -1;
    }
    pLine();

    // 5.1. 对边缘点应用基于Z高度的统计滤波
    if (!algorithm.applyEdgeKNNFilter()) {
        std::cerr << "警告: 边缘点Z高度统计滤波失败，继续后续处理" << std::endl;
    }
    pLine();  // 边缘点平滑
    if (!algorithm.applyEdgeSmoothing()) {
        std::cerr << "警告: 边缘点平滑失败，继续后续处理" << std::endl;
    }
    pLine();

    // 5.5. 计算边缘路径法向量（使用参数文件）
    if (!algorithm.computeEdgeNormals()) {
        std::cerr << "警告: 法向量计算失败，将跳过法向量保存" << std::endl;
    }
    pLine();
    // 5.55. 生成外侧打胶点（新的区域偏移处理）
    const AlgorithmParameters& params = algorithm.getParams();
    if (params.outer_spray_offset.enable_offset_adjustment) {//是否启用外部路径偏移
        std::cout << "开始生成外侧打胶点（包含区域偏移处理）..." << std::endl;
        if (!algorithm.generateOuterGluingPoints()) {
            std::cerr << "警告: 外侧打胶点生成失败" << std::endl;
        }
    } else {
        std::cout << "外部路径偏移调节已禁用，跳过外侧打胶点生成" << std::endl;
    }
    pLine();
    // 5.6. 生成内侧打胶点（使用参数文件）
    if (!algorithm.generateInnerGluingPoints()) {
        std::cerr << "警告: 内侧打胶点生成失败" << std::endl;
    }
    pLine();
    // 6. 保存结果
    algorithm.saveFilteredCloud("_filtered.pcd");
    algorithm.saveEdgePath("_gluing_path.pcd");
    algorithm.saveEdgePathWithNormalsPCD("_gluing_path_with_normals.pcd");
    algorithm.saveInnerGluingPointsPCD("_inner_gluing_points.pcd");
    // 保存外侧打胶点
    if (params.outer_spray_offset.enable_offset_adjustment) {
        algorithm.saveOuterGluingPointsPCD("_outer_gluing_points.pcd");
    }

    pLine();  // 7. 均匀采样路径（使用参数文件）
    if (!algorithm.uniformSamplePaths()) {
        std::cerr << "警告: 路径采样失败" << std::endl;
    } else {
        // 7.2 保存采样后的路径
        algorithm.saveSampledPaths("_sampled_outer_path.pcd", "_sampled_inner_path.pcd");
    }
    pLine();
    // 7.5. 计算路径点的角度信息并与路径点绑定
    std::cout << "开始计算路径点角度信息并绑定..." << std::endl;
    algorithm.processPathOrientations();
    std::cout << "路径角度信息处理完成" << std::endl;
    pLine();

    // 7.6. 创建全局清扫路径（合并外部和内部路径）
    std::cout << "开始创建全局清扫路径..." << std::endl;
    algorithm.createGlobalSweepPath();
    std::cout << "全局清扫路径创建完成" << std::endl;
    pLine();
    // 8. 应用标定变换（标定矩阵已在参数文件中加载）
    if (algorithm.transformSampledPaths()) {
        // 保存变换后的路径
        algorithm.saveTransformedPaths("_transformed_outer_path.pcd", "_transformed_inner_path.pcd", "_transformed_outer_path.txt",
                                       "_transformed_inner_path.txt");

        // 保存变换后的全局清扫路径
        algorithm.saveGlobalSweepPath("_global_sweep_path.txt");
    } else {
        std::cerr << "警告: 路径变换失败" << std::endl;
    }

    algorithm.socket_send_code();  // 发送socket状态码
    // 打印统计信息
    algorithm.printStatistics();

    std::cout << "=== 鞋底打胶路径算法完成 ===" << std::endl;

    return 0;
}
